﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using AForge.Imaging.Filters;
using AForge.Imaging;
using PositionSource.WebCam.Interfaces;
using RoverStudio.Math;

namespace PositionSource.LedPositionExtractor
{

    public class LedDetector : ILedPositionExtractor
    {
        // Number of LEDs on the rover
        private int numLEDs = 8;

        public int NumberOfLEDs
        {
            get { return numLEDs; }
            // set { numLEDs = value; }
        }

        // Average position of the LEDs on the picture
        private int framesToAverage = 1;

        public int FramesToAverage
        {
            get { return framesToAverage; }
            set { framesToAverage = value; }
        }

        private int count = 0;

        /// <summary>
        /// Averaged LED positions
        /// </summary>
        Vector2D[] avg_points;


        #region Image Processing Filters

        private FiltersSequence preprocessFilters = new FiltersSequence();
        //private IFilter grayscaleFilter = new BlueLoverGrayscale();
        // private IFilter grayscaleFilter = new GrayscaleY();

        private Threshold thresholdFilter = new Threshold(90);
        //        private Opening openingFiler = new Opening(new short[,] { { 0, 1, 1, 1, 0 }, { 1, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1 }, { 0, 1, 1, 1, 0 } });

        private FiltersSequence postprocessFilters = new FiltersSequence();


        private Edges edgesFilter = new Edges();

        private bool adaptiveTreshold = true;

        #endregion

        /// <summary>
        /// Integer - double pair for the convex hull algorithm
        /// </summary>
        struct idpair : IComparable
        {
            public idpair(int i, double ang)
            {
                this.i = i;
                this.ang = ang;
            }
            public int i;
            public double ang;

            #region IComparable Members

            public int CompareTo(object obj)
            {
                return ang.CompareTo(((idpair)obj).ang);
            }

            #endregion
        }

        /// <summary>
        /// Point type for identification of leds
        /// </summary>
        enum PointType
        {
            UNKNOWN, CORNER, EDGE, CENTER
        }

        public LedDetector()
        {
            avg_points = new Vector2D[numLEDs];
            for (int i = 0; i < numLEDs; i++)
                avg_points[i] = new Vector2D();
            //preprocessFilters.Add(new BrightnessCorrection());
            preprocessFilters.Add(new BlueLoverGrayscale());
            //preprocessFilters.Add(new Blur());
            //preprocessFilters.Add(new Blur());

            postprocessFilters.Add(thresholdFilter);
            postprocessFilters.Add(new Dilatation());
            //new Opening(new short[,] { { 1, 1, 1 }, { 1, 1, 1 }, { 1, 1, 1 } });

        }

        #region ILedPositionExtractor Members

        private Pen pen = new Pen(Color.Red, 3);
        private Graphics g;

        private Font myFont = new Font("Microsoft Sans Serif", 16);
        private Brush blueBrush = new SolidBrush(Color.Blue);
        private Brush whiteBrush = new SolidBrush(Color.White);
        private Brush redBrush = new SolidBrush(Color.Red);

        private Rectangle roverRectangle = new Rectangle(0, 0, 0, 0);

        public Vector2D[] GetLEDPositionsFromImage(ref Bitmap image)
        {

            if (count == 0)
            {
                for (int i = 0; i < numLEDs; i++)
                {
                    avg_points[i].X = 0;
                    avg_points[i].Y = 0;
                }
            }
            if (roverRectangle.Height == 0)
            {
                roverRectangle = new Rectangle(0, 0, image.Width, image.Height);
            }
            Bitmap croppedImage = new Bitmap(roverRectangle.Width, roverRectangle.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            Graphics croppedImageGr = Graphics.FromImage(croppedImage);
            croppedImageGr.DrawImage(image, 0, 0, roverRectangle, GraphicsUnit.Pixel);
            croppedImageGr.Dispose();

            Bitmap grayImage = preprocessFilters.Apply(croppedImage);

            // Threshold included
            Bitmap bwImage = postprocessFilters.Apply(grayImage);

            //Bitmap bwImage = openingFiler.Apply(bwImage1);
            //bwImage1.Dispose();

            Bitmap showImage = edgesFilter.Apply(bwImage);

            BlobCounter blobCounter = new BlobCounter();

            blobCounter.ProcessImage(bwImage);

            bwImage.Dispose();

            if (adaptiveTreshold)
            {
/*
                if (blobCounter.ObjectsCount < numLEDs && thresholdFilter.Min >= 20)
//                    thresholdFilter.Min -= 10;
                    thresholdFilter.Min += 10;
                else if (blobCounter.ObjectsCount > numLEDs && thresholdFilter.Min <= 240)
//                    thresholdFilter.Min += 10;
                    thresholdFilter.Min -= 10;
                */
                if (blobCounter.ObjectsCount != numLEDs)
                    thresholdFilter.ThresholdValue = (byte)((thresholdFilter.ThresholdValue + 10) % 250);
            }
            //g = Graphics.FromImage(image);
            //g.DrawString(String.Format("threshold = {0}", thresholdFilter.Min), myFont, Brushes.Blue, 0, 0);

            if (blobCounter.ObjectsCount != numLEDs)
            {
                roverRectangle = new Rectangle(0, 0, 0, 0);
//                g.Dispose();
                return null;// goto?
            }


            Vector2D[] points = new Vector2D[numLEDs];
            //            PointType[] pointtypes = new PointType[numLEDs];
            int[] wx = new int[numLEDs + 1];
            int[] wy = new int[numLEDs + 1];
            int[] w = new int[numLEDs + 1];

            for (int i = 0; i <= numLEDs; i++)
                wx[i] = wy[i] = w[i] = 0;

            int[] objectLabels = blobCounter.ObjectLabels;

            int width = grayImage.Width, height = grayImage.Height;
            for (int y = 0, i = 0; y < height; y++)
                for (int x = 0; x < width; x++, i++)
                {
                    int label = objectLabels[i];
                    if (label == 0) // background
                        continue;
                    byte pixel = grayImage.GetPixel(x, y).R;  //pixel value
                    wx[label] += x * pixel;
                    wy[label] += y * pixel;
                    w[label] += pixel;
                }

            grayImage.Dispose();

            //Graphics g = Graphics.FromImage(image);

            for (int i = 0; i < numLEDs; i++)
            {
                points[i] = new Vector2D(roverRectangle.X + ((double)wx[i + 1]) / w[i + 1], roverRectangle.Y + ((double)wy[i + 1]) / w[i + 1]);
                //g.DrawString(String.Format("({0:0000.0},{1:0000.0})", points[i].X, points[i].Y), myFont, whiteBrush, 0, i * 20);
            }

            // Changing RoverRectangle:

            double minX = image.Width;
            double minY = image.Height;
            double maxX = 0.0;
            double maxY = 0.0;
            for (int i = 0; i < numLEDs; i++)
            {
                if (minX > points[i].X)
                    minX = points[i].X;
                if (minY > points[i].Y)
                    minY = points[i].Y;
                if (maxX < points[i].X)
                    maxX = points[i].X;
                if (maxY < points[i].Y)
                    maxY = points[i].Y;
            }
            minX = Math.Max(0, minX - (maxX - minX) / 2);
            minY = Math.Max(0, minY - (maxY - minY) / 2);
            maxX = Math.Min(image.Width - 1, maxX + (maxX - minX) / 2);
            maxY = Math.Min(image.Height - 1, maxY + (maxY - minY) / 2);

            roverRectangle = new Rectangle((int)minX, (int)minY, (int)(maxX - minX), (int)(maxY - minY));

            try
            {
                //g = Graphics.FromImage(image);
                g = Graphics.FromImage(image);
                g.DrawString(String.Format("threshold = {0}", thresholdFilter.ThresholdValue), myFont, Brushes.Blue, 0, 0);
                g.DrawRectangle(Pens.White, roverRectangle);
                //g.DrawString(String.Format("threshold = {0}", thresholdFilter.Min), myFont, Brushes.Blue, 0, 0);
                sortPoints(ref points);

                for (int i = 0; i < numLEDs; i++)
                {
                    g.DrawEllipse(Pens.Red, (int)(points[i].X - 10), (int)(points[i].Y - 10), 21, 21);
                    g.DrawString(String.Format("{0}", i), myFont, Brushes.Blue, (float)(points[i].X), (float)(points[i].Y-10));
                }
                g.Dispose();

                image = showImage;

                for (int i = 0; i < numLEDs; i++)
                    avg_points[i].Shift(points[i]);

                if (++count == framesToAverage)
                {
                    count = 0;
                    for (int i = 0; i < numLEDs; i++)
                        avg_points[i].Scale(1.0 / framesToAverage);
                    return avg_points;
                }
            }
            catch (Exception)
            {
                roverRectangle = new Rectangle(0, 0, 0, 0);

                return null;
            }
            return null;
        }

        #endregion

        private void sortPoints(ref Vector2D[] points)
        {
            PointType[] pointtypes = new PointType[numLEDs];
            for (int i = 0; i < numLEDs; i++)
                pointtypes[i] = PointType.UNKNOWN;

            List<idpair> angles = new List<idpair>();
            int pminyx = 0; // index of Point with minimal y value (and minimal x value in case of equality)
            for (int i = 1; i < numLEDs; i++)
                if (points[i].Y < points[pminyx].Y || points[i].Y == points[pminyx].Y && points[i].X < points[pminyx].X)
                {
                    pminyx = i;
                }
            // Move the lower left point to index 0
            Vector2D ptmp = points[0];
            points[0] = points[pminyx];
            points[pminyx] = ptmp;

            angles.Add(new idpair(0, 0));
            for (int i = 1; i < numLEDs; i++)
            {
                Vector2D diff = points[i] - points[0];
                double angle = Math.Atan2(diff.Y, diff.X);
                angles.Add(new idpair(i, angle));
            }

            angles.Sort(1, numLEDs - 1, null);
            List<int> convexhull = new List<int>(numLEDs); // Point indices! of convex hull
            convexhull.Add(angles[0].i);
            convexhull.Add(angles[1].i);



            for (int i = 2; i < numLEDs; i++)
            {
                while (((points[convexhull[convexhull.Count - 1]] - points[convexhull[convexhull.Count - 2]]) ^
                    (points[angles[i].i] - points[convexhull[convexhull.Count - 2]])) < 0)
                    convexhull.RemoveAt(convexhull.Count - 1);
                convexhull.Add(angles[i].i);
            }
            // Counterclockwise convex hull ready


            //for (int i = 0; i < convexhull.Count; i++)
            //{
            //    //                g.DrawString(String.Format("({0:0000.0}; {1:0000.0})", (float)points[convexhull[i]].X, (float)points[convexhull[i]].Y), myFont, whiteBrush, (float)points[convexhull[i]].X, (float)points[convexhull[i]].Y);
            //    g.DrawString(String.Format("({0};{1})", i, convexhull[i]), myFont, whiteBrush, (float)points[convexhull[i]].X, (float)points[convexhull[i]].Y);

            //}

            int[] corners = new int[4];
            int[] edgecenters = new int[4];
            int convexhullCount = convexhull.Count;
            // Add 2 more points
            convexhull.Add(convexhull[0]);
            convexhull.Add(convexhull[1]);
            int ci = 0, ei = 3; // Corners and edges index
            for (int i = 0; i < 4; i++)
                edgecenters[i] = -1;
            bool nextcorner = false;
            try
            {
                for (int i = 1; i <= convexhullCount; i++)
                {
                    if (nextcorner || Math.Abs(angle(points[convexhull[i]], points[convexhull[i - 1]], points[convexhull[i + 1]])) < Math.PI / 180 * (180 - 15))
                    {   // corner
                        nextcorner = false;
                        corners[ci] = convexhull[i];
                        pointtypes[convexhull[i]] = PointType.CORNER;
                        ei = (ci++) % 4;
                    }
                    else  //edgecenter
                    {
                        nextcorner = true;
                        edgecenters[ei] = convexhull[i];
                        pointtypes[convexhull[i]] = PointType.EDGE;
                    }
                }
            }
            catch (IndexOutOfRangeException)
            {
                throw new Exception("More than 4 corner points found!");
            }

            if (ci != 4) // Could not find 4 corner points
                throw new Exception("Could not find 4 corner points!");


            //            for (int i = 0; i < 4; i++)
            //            {
            //                g.DrawString("C"+i, myFont, whiteBrush, points[corners[i]].X, points[corners[i]].Y-10);
            //            }

            for (int i = 0; i < 4; i++)
            {
                if (edgecenters[i] == -1) // Not found jet
                {
                    for (int j = 0; j < numLEDs; j++)
                    {
                        if (pointtypes[j] != PointType.UNKNOWN)
                            continue;
                        if (Math.Abs(angle(points[j], points[corners[i]], points[corners[(i + 1) % 4]])) > Math.PI / 180 * (180 - 15))
                        {   // Point points[j] is the edge center between corners #i and #(i+1)
                            edgecenters[i] = j;
                            pointtypes[j] = PointType.EDGE;
                            break;
                        }
                    }
                }

                //g.DrawString("E" + i, myFont, whiteBrush, points[edgecenters[i]].X, points[edgecenters[i]].Y - 10);
            }


            Vector2D[] sortedPoints = new Vector2D[numLEDs];

            //Finding the center point
            int centercnt = 0;
            for (int j = 0; j < numLEDs; j++)
            {
                if (pointtypes[j] != PointType.UNKNOWN)
                    continue;
                pointtypes[j] = PointType.CENTER;
                sortedPoints[0] = points[j];
                centercnt++;
            }
            if (centercnt != 1)
                throw new Exception("Found potentional centers' count is " + centercnt + "!");

            // Identifying first corner by the missing edgecenter
            int firstcorner = 0;
            while (edgecenters[firstcorner] != -1)
                firstcorner++;
            firstcorner = (firstcorner + 1) % 4;

            int index = 1;
            for (int i = 0; i < 3; i++)
            {
                sortedPoints[index++] = points[corners[firstcorner]];
                sortedPoints[index++] = points[edgecenters[firstcorner]];
                firstcorner = (firstcorner + 1) % 4;
            }
            sortedPoints[index++] = points[corners[firstcorner]]; //Last corner

            points = sortedPoints;
        }

        private double angle(Vector2D a, Vector2D b, Vector2D c)
        {
            Vector2D ab = b - a;
            Vector2D ac = c - a;
            return Math.Atan2(ab ^ ac, ab * ac);
        }

    }
}
